/*
 *  Startup Code for MIPS32 XBURST CPU-core
 *
 *  Copyright (c) 2010 Xiangfu Liu <xiangfu@sharism.cc>
 *
 * See file CREDITS for list of people who contributed to this
 * project.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 */

#include <config.h>
#include <version.h>
#include <asm/regdef.h>
#include <asm/mipsregs.h>
#include <asm/addrspace.h>
#include <asm/cacheops.h>
#include <asm/arch/base.h>
#include "traps.h"




	.set noreorder
	.set mips32r2

	.globl _start
	.text
_start:
#if (defined(CONFIG_X2000_V12) || defined(CONFIG_M300))
#ifdef CONFIG_JZ_SECURE_SUPPORT
	.space 2048, 0

#endif
#endif

#ifdef _DEBUG

	la      v0, 0xb0030000
	li	t0, '0'
	sw 	t0, 0(v0)
loop0:
	lbu 	v1, 0x14(v0)
	li	t1, 0x60
	andi	v1, v1, 0x60
	bne 	v1, t1, loop0
	nop
	nop
#endif
	/* Initialize $gp */
	bal	1f
	 nop
	.word	_gp
1:
	lw	gp, 0(ra)
#ifdef _DEBUG


	la      v0, 0xb0030000
	li	t0, '1'
	sw 	t0, 0(v0)
loop1:
	lbu 	v1, 0x14(v0)
	andi	v1, v1, 0x60
	bne 	v1, 0x60, loop1
	nop
	nop
#endif
	/* Set up temporary stack */
	li	sp, CONFIG_SYS_SDRAM_BASE + CONFIG_SYS_INIT_SP_OFFSET
#ifdef _DEBUG
	la      v0, 0xb0030000
	li	t0, '2'
	sw 	t0, 0(v0)
loop2:
	lbu 	v1, 0x14(v0)
	andi	v1, v1, 0x60
	bne 	v1, 0x60, loop2
	nop
	nop
#endif
	la	t9, board_init_f

#ifdef _DEBUG
	la      v0, 0xb0030000
	li	t0, '3'
	sw 	t0, 0(v0)
loop3:
	lbu	v1, 0x14(v0)
	andi	v1, v1, 0x60
	bne 	v1, 0x60, loop3
	nop
	nop
#endif
	jr	t9
	 nop

/*
 * void relocate_code (addr_sp, gd, addr_moni)
 *
 * This "function" does not return, instead it continues in RAM
 * after relocating the monitor code.
 *
 * a0 = addr_sp
 * a1 = gd
 * a2 = destination address
 */
	.globl	relocate_code
	.ent	relocate_code
relocate_code:
	move	sp, a0			# set new stack pointer

	li	t0, CONFIG_SYS_MONITOR_BASE
	sub	t6, a2, t0		# t6 <-- relocation offset

	la	t3, in_ram
	lw	t2, -12(t3)		# t2 <-- __image_copy_end
	move	t1, a2

	add	gp, t6			# adjust gp

	/*
	 * t0 = source address
	 * t1 = target address
	 * t2 = source end address
	 */
1:
	lw	t3, 0(t0)
	sw	t3, 0(t1)
	addu	t0, 4
	blt	t0, t2, 1b
	 addu	t1, 4

	/* If caches were enabled, we would have to flush them here. */

	/* flush d-cache */
	li	t0, KSEG0
	or	t1, t0, CONFIG_SYS_DCACHE_SIZE
2:
	cache	INDEX_WRITEBACK_INV_D, 0(t0)
	bne	t0, t1, 2b
	 addi	t0, CONFIG_SYS_CACHELINE_SIZE

	sync

	/* flush i-cache */
	li	t0, KSEG0
	or	t1, t0, CONFIG_SYS_ICACHE_SIZE
3:
	cache	INDEX_INVALIDATE_I, 0(t0)
	bne	t0, t1, 3b
	 addi	t0, CONFIG_SYS_CACHELINE_SIZE

	/* Invalidate BTB */
	mfc0	t0, CP0_CONFIG, 7
	nop
	ori	t0, 2
	mtc0	t0, CP0_CONFIG, 7
	nop

	/* Jump to where we've relocated ourselves */
	addi	t0, a2, in_ram - _start
	jr	t0
	 nop

	.word	__rel_dyn_end
	.word	__rel_dyn_start
	.word	__image_copy_end
	.word	_GLOBAL_OFFSET_TABLE_
	.word	num_got_entries

in_ram:
	/*
	 * Now we want to update GOT.
	 *
	 * GOT[0] is reserved. GOT[1] is also reserved for the dynamic object
	 * generated by GNU ld. Skip these reserved entries from relocation.
	 */
	lw	t3, -4(t0)		# t3 <-- num_got_entries
	lw	t4, -8(t0)		# t4 <-- _GLOBAL_OFFSET_TABLE_
	add	t4, t6			# t4 now holds relocated _G_O_T_
	addi	t4, t4, 8		# skipping first two entries
	li	t2, 2
1:
	lw	t1, 0(t4)
	beqz	t1, 2f
	 add	t1, t6
	sw	t1, 0(t4)
2:
	addi	t2, 1
	blt	t2, t3, 1b
	 addi	t4, 4

	/* Update dynamic relocations */
	lw	t1, -16(t0)		# t1 <-- __rel_dyn_start
	lw	t2, -20(t0)		# t2 <-- __rel_dyn_end

	b	2f			# skip first reserved entry
	 addi	t1, 8

1:
	lw	t3, -4(t1)		# t3 <-- relocation info

	sub	t3, 3
	bnez	t3, 2f			# skip non R_MIPS_REL32 entries
	 nop

	lw	t3, -8(t1)		# t3 <-- location to fix up in FLASH

	lw	t4, 0(t3)		# t4 <-- original pointer
	add	t4, t6			# t4 <-- adjusted pointer

	add	t3, t6			# t3 <-- location to fix up in RAM
	sw	t4, 0(t3)

2:
	blt	t1, t2, 1b
	 addi	t1, 8			# each rel.dyn entry is 8 bytes

	/*
	 * Clear BSS
	 *
	 * GOT is now relocated. Thus __bss_start and __bss_end can be
	 * accessed directly via $gp.
	 */
	la	t1, __bss_start		# t1 <-- __bss_start
	la	t2, __bss_end		# t2 <-- __bss_end

1:
	sw	zero, 0(t1)
	blt	t1, t2, 1b
	 addi	t1, 4

	move	a0, a1			# a0 <-- gd
	la	t9, board_init_r
	jr	t9
	 move	a1, a2

	.end	relocate_code


#ifdef CONFIG_XBURST_TRAPS
// push context: at, v0-v1,a0-a3,t0-t9,s0-s7,fp,ra, & pc
	.macro  SAVE_ALL
	.set noat
	.set noreorder
        sw ra,PT_RA(sp)
        sw fp,PT_FP(sp)
        sw t9,PT_T9(sp)
        sw t8,PT_T8(sp)
        sw s7,PT_S7(sp)
        sw s6,PT_S6(sp)
        sw s5,PT_S5(sp)
        sw s4,PT_S4(sp)
        sw s3,PT_S3(sp)
        sw s2,PT_S2(sp)
        sw s1,PT_S1(sp)
        sw s0,PT_S0(sp)
        sw t7,PT_T7(sp)
        sw t6,PT_T6(sp)
        sw t5,PT_T5(sp)
        sw t4,PT_T4(sp)
        sw t3,PT_T3(sp)
        sw t2,PT_T2(sp)
        sw t1,PT_T1(sp)
        sw t0,PT_T0(sp)
        sw a3,PT_A3(sp)
        sw a2,PT_A2(sp)
        sw a1,PT_A1(sp)
        sw a0,PT_A0(sp)
        sw v1,PT_V1(sp)
        sw v0,PT_V0(sp)
        sw AT,PT_AT(sp)
        mfc0 k1,CP0_STATUS
        sw k1,PT_STATUS(sp)
        mfc0 k1, CP0_EPC
        sw k1,PT_EPC(sp)
        mflo k1
        sw k1,PT_LO(sp)
        mfhi k1
        sw k1,PT_HI(sp)
	nop
	la t1,0x80000004
	sw sp,0(t1)
	.endm

#define	NESTED(symbol, framesize, rpc)                  \
		.globl	symbol;                         \
		.align	2;                              \
		.type	symbol, @function;              \
		.ent	symbol, 0;                       \
symbol:		.frame	sp, framesize, rpc

/*
 * END - mark end of function
 */
#define	END(function)                                   \
		.end	function;		        \
		.size	function, .-function


/*
 * General exception vector for all other CPUs.
 *
 * Be careful when changing this, it has to be at most 128 bytes
 * to fit into space reserved for the exception handler.
 */
NESTED(handle_exception, 0, sp)
	addi sp,sp,-PT_LEN
	SAVE_ALL
	la	t9,0x80000000
	lw	t9,0(t9)
	jr	t9
	nop
	nop
END(handle_exception)

#endif
